clc;
clear;
warning off;

% Load data from the Excel file
filename = 'Trial0001_6D.xls';
data = readtable(filename);

% Define constants
armLength = 1.8 * 1000; % Arm length in millimeters
startFrame = 1;         % Start processing from Frame 1

% Extract relevant columns
frame = data{:, 1};
time = data{:, 2};
SMPTE = data{:, 3};
westHeight = data{:, 6};
centralHeight = data{:, 23};
centralPitchValues = data{:, 25};

% Find start index for Frame 1000
startIndex = find(frame >= startFrame, 1);

% Filter out rows where westHeight is zero
validIndices = westHeight ~= 0;
frame = frame(validIndices);
time = time(validIndices);
SMPTE = SMPTE(validIndices);
westHeight = westHeight(validIndices);
centralHeight = centralHeight(validIndices);
centralPitchValues = centralPitchValues(validIndices);


% Plot westHeight data to visualize its structure and variability
figure;
plot(westHeight);
title('Trial 7 - West Float Height');
xlabel('Frame');
ylabel('West Float Height');
grid on;

try
    [peaks, peakLocs] = findpeaks(westHeight);
    [troughs, troughLocs] = findpeaks(-westHeight);
catch
    error('Error: Unable to find peaks or troughs in the westHeight data. Check data variability.');
end

% Check if peaks and troughs were found
if isempty(peakLocs) || isempty(troughLocs)
    error('No peaks or troughs found in the westHeight data within the specified range.');
end


disp('Processing complete.');
